package com.googlecode.curvedmetromaps.core.logic.forcedriven;

import org.jbox2d.common.Vec2;
import org.jbox2d.dynamics.Body;

import com.googlecode.curvedmetromaps.core.geometry.BasicGeometry;
import com.googlecode.curvedmetromaps.core.model.MetroStation;

/**
 * Constraint S8: geographic constraint
 */
public class ForceGeographicAnglePreservation extends ForceSubsystem {
	public ForceWeight freedom = new ForceWeight("Angular freedom", 0.25 * Math.PI);

	float[][] geoAngles;
	
	ForceGeographicAnglePreservation(ForceSystem system) {
		super(system);
		
		baseWeight.value = 500;
		
		MetroStation[] stations = system.metroMap.stations;
		geoAngles = new float[stations.length][stations.length];
		for (MetroStation m1 : stations) {
			for (MetroStation m2 : stations) {
				if (m1.id < m2.id) {
					// Vector from geographical positions of m1 to m2
					Vec2 g12 = new Vec2(m2.geoX - m1.geoX, m2.geoY - m1.geoY);
					// Calculate angle
					geoAngles[m1.id][m2.id] = BasicGeometry.atan2(g12.y, g12.x);
				}
			}
		}
	}

	void applyForces(int delta) {
		for (MetroStation m1 : system.metroMap.stations) {
			for (MetroStation m2 : system.metroMap.stations) {
				if (m1.id < m2.id) {
					Body b1 = system.stationBodies[m1.id];
					Body b2 = system.stationBodies[m2.id];
					Vec2 p1 = b1.getPosition();
					Vec2 p2 = b2.getPosition();
					// Vector from distorted positions of m1 to m2
					Vec2 v12 = p2.sub(p1);

					// Calculate current angle by approximation.
					float vAngle = BasicGeometry.atan2(v12.y, v12.x);
					float gAngle = geoAngles[m1.id][m2.id];

					// Calculate difference between drawing and geographical
					// angles.
					float deltaAngle = vAngle - gAngle;
					if (deltaAngle <= -Math.PI)
						deltaAngle += 2. * Math.PI;
					else if (deltaAngle > Math.PI)
						deltaAngle -= 2. * Math.PI;

					// Calculate difference between equilibrium angle and
					// deltaAngle
					double theta = Math.abs(deltaAngle) - freedom.value;
					if (theta > 0) {
						Vec2 f = new Vec2(-v12.y, v12.x);
						double mag = baseWeight.value * theta * theta;
						f.mulLocal((float) (mag * Math.signum(deltaAngle)));
						b1.applyForceToCenter(f);
						b2.applyForceToCenter(f.negateLocal());
						system.metroMap.geoForces[m1.id][m2.id] = f.length();
					} else {
						system.metroMap.geoForces[m1.id][m2.id] = 0f;
					}
				}
			}
		}
	}
}
